/*
 * Car.cpp
 *
 * Created: 2012-05-16 10:04:11
 *  Author: Johan
 */ 
#include "Car.h"

Car::Car(PWM p, UART u):
uart(u), pwm(p)
{
	wheel_forward = 1300;
	wheel_right = 1200;
	wheel_left = 1450;
	
	motor_stop = 1500;
	motor_min = 1700;
	motor_max = 1300;
	
	motor_start_delay_limit = 500;
	motor_start_delay_current = 0;
	
	braking_force = 0;
	braking_time_limit = 0;
	braking_time_current = 0;
	
	braking_time_begin.current = 0;
	braking_time_begin.limit = 1000;
	
	braking_time_idle.current = 0;
	braking_time_idle.limit = 1000;
	
	braking_time.current = 0;
	braking_time.limit = 0;
	
	car_speed = motor_stop;
	wheel_angle = wheel_forward;

}

Car::State Car::getState(){
	return state;
}

void Car::wheelForward() {
	wheel_angle = wheel_forward;
}

void Car::move(){
	pwm.setWidth(pwm.C, car_speed);
	pwm.setWidth(pwm.B, wheel_angle);
}

void Car::wheelTurn(int turn) {
	
	int wheel_turn = wheel_forward + ((turn)*0.25) ;
	
	if (wheel_turn > wheel_left) {
		wheel_turn = wheel_left;
	} else if (wheel_turn < wheel_right) {
		wheel_turn = wheel_right;
	}
	
	wheel_angle = wheel_turn;
}

void Car::brake(int speed, int ms){
	if(state == Running){
		state = Braking;
		car_speed = speed;
		braking_time.limit = ms;
	}
}

void Car::start() {
	state = Starting;
	uart.send("Engine starting");
}

void Car::stop(){
	state = Idle;
	car_speed = motor_stop;
	motor_start_delay_current = 0;
	uart.send("Engine stop\n\r");
}

void Car::onBraking(int ms){
	//if(car_speed<motor_min){
		//car_speed = motor_min;
	//}
	//braking_time_begin.current++;
	//if(braking_time_begin.current>braking_time_begin.limit){
		//braking_time_idle.current++;
		//car_speed += 1;
		//if(car_speed>motor_stop){
			//car_speed = motor_stop;
		//}
		//if(braking_time_idle.current>braking_time_idle.limit){
			//braking_time.current++;
			//car_speed -= 1;
			//if(braking_force>car_speed){
				//car_speed = braking_force;
			//}
			//if(braking_time.current>braking_time.limit){
				//stop();
			//}
		//}
	//}
	
	braking_time.current++;
	if(braking_time.current>braking_time.limit){
		stop();
		braking_time_current = 0;
	}
}

void Car::update(int ms){
	
	switch(state){
		case Starting:
			onStarting(ms);
			break;
		case Running:
			
			break;
		case Braking:
			onBraking(ms);
			break;
		case Idle:
			stop();
			break;
		default:
			break;
	}
}

void Car::onStarting(int ms){
	motor_start_delay_current += ms;
	if(motor_start_delay_current>motor_start_delay_limit){
		state = Running;
		motor_start_delay_current = 0;
		uart.send("Engine started\n\r");
	}
}

int Car::getSpeed(){
	return car_speed;
}

void Car::accelerate(int speed){
	setSpeed(getSpeed()+speed);
}

void Car::setSpeed(int speed){
	
	if(state == Running){
		if (speed > motor_max) {
			speed = motor_max;
		} else if (speed < motor_min) {
			speed = motor_min;
		}
		
		car_speed = speed;
	}
}


